#include "pch.h"
#include "hci_comm.h"
#include "hci.h"
#include "hid.h"
#include "l2cap.h"
#include <stdio.h>

DEVICE_DATA deviceData;
UCHAR eventBuffer[255];
unsigned long eventBufferLength;



uint8_t blue_open()
{
	HRESULT               hr;
	USB_DEVICE_DESCRIPTOR deviceDesc;
	BOOL                  bResult;
	BOOL                  noDevice;
	ULONG                 lengthReceived;

	// Find a device connected to the system that has WinUSB installed using our INF
	hr = OpenDevice(&deviceData, &noDevice);

	if (FAILED(hr)) {
		if (noDevice) {
			printf(_T("Device not connected or driver not installed\n"));
		}
		else {
			printf(_T("Failed looking for device, HRESULT 0x%x\n"), hr);
		}
		return 1;
	}

	// Get device descriptor
	bResult = WinUsb_GetDescriptor(deviceData.WinusbHandle,
		USB_DEVICE_DESCRIPTOR_TYPE,
		0,
		0,
		(PBYTE)&deviceDesc,
		sizeof(deviceDesc),
		&lengthReceived);

	if (FALSE == bResult || lengthReceived != sizeof(deviceDesc)) {
		printf(_T("Error among LastError %d or lengthReceived %d\n"),
			FALSE == bResult ? GetLastError() : 0,
			lengthReceived);
		CloseDevice(&deviceData);
		return 1;
	}

	// Print a few parts of the device descriptor
	printf(_T("Device found: VID_%04X&PID_%04X; bcdUsb %04X\n"),
		deviceDesc.idVendor,
		deviceDesc.idProduct,
		deviceDesc.bcdUSB);


	// Interface description, should be class 0xE0 Wireless Controller,
	// subclass 0x01 and protocol 0x01 Bluetooth Programming Interface
	// http://www.usb.org/developers/defined_class/#BaseClassE0h
	USB_INTERFACE_DESCRIPTOR interfaceDesc;

	bResult = WinUsb_QueryInterfaceSettings(deviceData.WinusbHandle, 0, &interfaceDesc);
	if (FALSE == bResult) {

		printf(_T("Error among LastError %d \n"),
			FALSE == bResult ? GetLastError() : 0);
		CloseDevice(&deviceData);
		return 1;
	}

	return 0;
}

uint8_t send_command(unsigned char *pCmd, unsigned short size)
{
	WINUSB_SETUP_PACKET SetupPacket;
	ZeroMemory(&SetupPacket, sizeof(WINUSB_SETUP_PACKET));
	SetupPacket.RequestType = (BMREQUEST_HOST_TO_DEVICE << 7) | (BMREQUEST_CLASS << 5) | BMREQUEST_TO_DEVICE;
	SetupPacket.Index = 0; // endpoint
	SetupPacket.Request = 0; // HCI
	SetupPacket.Value = 0;
	SetupPacket.Length = 3;

	print_command(pCmd, (uint8_t)size);
	unsigned long transferred;
	BOOL ok = WinUsb_ControlTransfer(deviceData.WinusbHandle, SetupPacket, pCmd, size, &transferred, NULL);
	if (!ok) return 2;

	return 0;
}

uint8_t read_event()
{
	BOOL ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x81, eventBuffer, 255, &eventBufferLength, NULL);
	if (!ok) return 1;
	print_event(eventBuffer, (uint8_t)eventBufferLength);
	return 0;
}


uint8_t blue_reset()
{
	uint8_t result;
	unsigned char *pCmd;
	unsigned short size;
	
	WinUsb_FlushPipe(deviceData.WinusbHandle, 0x02);
	WinUsb_FlushPipe(deviceData.WinusbHandle, 0x81);
	WinUsb_FlushPipe(deviceData.WinusbHandle, 0x82);

	get_reset_command(&pCmd, &size);
	result = send_command(pCmd, size);
	if (result != 0) return 1;

	bool complete = false;
	while (!complete)
	{
		result = read_event();
		if (result != 0) return 1;
		if (eventBuffer[0] != 0x0E) continue;
		if (*((uint16_t*)&eventBuffer[3]) != *((uint16_t*)pCmd)) continue;
		complete = true;
	}
	if (eventBuffer[5] != 0) return 2;

	return 0;
}

uint8_t blue_scan_enable()
{
	uint8_t result;
	unsigned char *pCmd;
	unsigned short size;

	get_scan_enable_command(&pCmd, &size, 2);
	result = send_command(pCmd, size);
	if (result != 0) return 1;

	bool complete = false;
	while (!complete)
	{
		result = read_event();
		if (result != 0) return 1;
		if (eventBuffer[0] != 0x0E) continue;
		if (*((uint16_t*)&eventBuffer[3]) != *((uint16_t*)pCmd)) continue;
		complete = true;
	}
	if (eventBuffer[5] != 0) return 2;

	return 0;
}

uint8_t blue_connect(uint16_t *handle)
{
	uint8_t result;
	unsigned char *pCmd;
	unsigned short size;


	bool receivedRequest = false;
	while (!receivedRequest)
	{
		result = read_event();
		if (result!=0) return 1;
		if (eventBuffer[0] != 0x04) continue;
		get_accept_connection_request_command(&pCmd, &size, &(eventBuffer[2]) /*addr*/, 0 /*master*/);
		result = send_command(pCmd, size);
		if (result != 0) return 1;
		receivedRequest = true;
	}

	bool complete = false;
	while (!complete)
	{
		result = read_event();
		if (result != 0) return 1;
		if (eventBuffer[0] != 0x03) continue;
		complete = true;
	}
	if (eventBuffer[2] != 0) return 2;

	*handle = *((uint16_t*)&eventBuffer[3]);

	// wieder weg
	read_event();
	read_event();


	return 0;
}

uint8_t blue_setup_l2cap(uint8_t channel_id)
{
	uint8_t *pCmd;
	uint8_t size;
	ULONG transferred;
	l2cap_connection connection;
	l2cap_connection connection_config;

	BOOL ok = TRUE;
	BOOL connection_request = FALSE;
	while (ok && !connection_request){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		connection_request = (eventBuffer[8]==0x02);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}

	connection.handle_low = eventBuffer[0];
	connection.handle_high = eventBuffer[1];
	connection.dcid_low = eventBuffer[14];
	connection.dcid_high = eventBuffer[15];
	connection.scid_low = channel_id;
	connection.scid_high = 0x00;
	connection.current_id = eventBuffer[9];


	printf("pending\n");
	get_l2cap_connection_pending_response(&connection, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;

	printf("complete\n");
	get_l2cap_connection_successful_response(&connection, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;


	printf("send configuration request\n");
	get_l2cap_configuration_request(&connection, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;

	printf("read configuration request\n");
	BOOL configuration_request = FALSE;
	while (ok && !configuration_request){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		configuration_request = (eventBuffer[8] == 0x04);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}

	connection_config.handle_low = eventBuffer[0];
	connection_config.handle_high = eventBuffer[1];
	connection_config.scid_low = eventBuffer[12];
	connection_config.scid_high = eventBuffer[13];
	connection_config.current_id = eventBuffer[9];

	printf("send configuration response\n");
	get_l2cap_configuration_response(&connection_config, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;


	printf("read configuration response\n");
	BOOL configuration_response = FALSE;
	while (ok && !configuration_response){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		configuration_response = (eventBuffer[8] == 0x05);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}

	read_event();
	read_event();
	read_event();
	read_event();

	printf("finished l2cap connection setup\n");

	return 0;
}

uint8_t blue_sdp(uint16_t handle)
{
	uint8_t *pCmd;
	uint8_t size;
	ULONG transferred;
	l2cap_connection connection;
	l2cap_connection connection_config;

	BOOL ok = TRUE;

	connection.handle_low = handle & 0xFF;
	connection.handle_high = handle >> 8;
	connection.dcid_low = 0x00;
	connection.dcid_high = 0x00;
	connection.scid_low = 0x50;
	connection.scid_high = 0x00;
	connection.current_id = 0;


	printf("send connection request\n");
	get_l2cap_connection_request(&connection, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;

	printf("read connection response\n");
	BOOL connection_response = FALSE;
	while (ok && !connection_response){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		connection_response = (eventBuffer[8] == 0x03) && (eventBuffer[16] == 0x00);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}

	connection.dcid_low = eventBuffer[12];
	connection.dcid_high = eventBuffer[13];
	printf("send configuration request\n");
	get_l2cap_configuration_request_mtu(&connection, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;

	printf("read configuration response\n");
	BOOL configuration_response = FALSE;
	while (ok && !configuration_response){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		configuration_response = (eventBuffer[8] == 0x05);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}

	printf("read configuration request\n");
	BOOL configuration_request = FALSE;
	while (ok && !configuration_request){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		configuration_request = (eventBuffer[8] == 0x04);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}


	connection_config.handle_low = eventBuffer[0];
	connection_config.handle_high = eventBuffer[1];
	connection_config.scid_low = eventBuffer[12];
	connection_config.scid_high = eventBuffer[13];
	connection_config.current_id = eventBuffer[9];

	printf("send configuration response\n");
	get_l2cap_configuration_response(&connection_config, &pCmd, &size);
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;

	read_event();
	read_event();
	read_event();


	uint8_t packet_buffer[32];
	uint8_t length = 21;
	packet_buffer[0] = handle & 0xFF;
	packet_buffer[1] = handle >> 8;
	packet_buffer[2] = length - 4;
	packet_buffer[3] = 0x00;
	packet_buffer[4] = length - 8;
	packet_buffer[5] = 0x00;
	packet_buffer[6] = connection.dcid_low;
	packet_buffer[7] = connection.dcid_high;

	packet_buffer[8] = 0x02; /*search*/
	packet_buffer[9] = 0x0F; /*transaction id*/
	packet_buffer[10] = 0x00;
	packet_buffer[11] = 0x08; /*parameter length*/
	packet_buffer[12] = 0x00;

	packet_buffer[13] = 0x06 << 3 | 0x05;  /*sequence*/
	packet_buffer[14] = 0x03; /*3 octets*/
	packet_buffer[15] = 0x03 << 3 | 0x01; /*UUID 2 octets*/
	packet_buffer[16] = 0x12; /*class*/
	packet_buffer[17] = 0x00;
	packet_buffer[18] = 0x03; /*max service response count*/
	packet_buffer[19] = 0x00;
	packet_buffer[20] = 0x00; /*continuation*/


	printf("send sdp request\n");
	ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, packet_buffer, length, &transferred, NULL);
	if (!ok) return 1;

	read_event();


	printf("read sdp response\n");
	//read_event();
	BOOL sdp_response = FALSE;
	while (ok && !sdp_response){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		sdp_response = (eventBuffer[8] == 0x03);
		print_acl(eventBuffer, (uint8_t)eventBufferLength);
	}
	printf("finished sdp\n");

	return 0;
}

uint8_t blue_listen_hid()
{
	uint8_t *pCmd;
	uint8_t size;
	ULONG transferred;

	printf("set hid enable\n");
	get_hid_enable(0x202A, &pCmd, &size);
	BOOL ok = WinUsb_WritePipe(deviceData.WinusbHandle, 0x02, pCmd, size, &transferred, NULL);
	if (!ok) return 1;

	printf("listen\n");
	while (ok){
		ok = WinUsb_ReadPipe(deviceData.WinusbHandle, 0x82, eventBuffer, 255, &eventBufferLength, NULL);
		if (!ok) return 1;
		//print_acl(eventBuffer, (uint8_t)eventBufferLength);

		PS3_data* data = (PS3_data*)&eventBuffer[8];
		printf("0x%02X, 0x%02X, 0x%02X\n", data->report_id, data->ps3_union.ps3_report.LAnalogX, data->ps3_union.ps3_report.Circle);
	}

	return 0;
}

void blue_close()
{
	CloseDevice(&deviceData);
}